local sharedLibs = game.ReplicatedStorage.SharedLibs;
local oo = require(sharedLibs.BaseLib)

local floor = math.floor

---
local PriorityQueue = oo.class();

function PriorityQueue:constructor()
	self.heap_val = {}
	self.heap_pri = {}
	self.current_size = 0
end

function PriorityQueue:Empty()
	return self.current_size == 0
end

function PriorityQueue:Size()
	return self.current_size
end

function PriorityQueue:Put(v, p)
  --[[ Put an item on the queue.
    Args:
        v: the item to be stored
        p(number): the priority of the item
    ]]--
	--
	self.current_size = self.current_size + 1
	self.heap_val[self.current_size] = v
	self.heap_pri[self.current_size] = p
	self:_swim()
end

function PriorityQueue:Pop()
	-- Remove and return the top priority item
	local heap_val = self.heap_val
	local heap_pri = self.heap_pri
	local retval, retprio = heap_val[1], heap_pri[1]
	heap_val[1], heap_pri[1] = heap_val[self.current_size], heap_pri[self.current_size]
	heap_val[self.current_size], heap_pri[self.current_size] = nil, nil
	self.current_size = self.current_size - 1
	self:_sink()
	return retval, retprio
end

function PriorityQueue:Peek()
	-- return the top priority item
	return self.heap_val[1], self.heap_pri[1]
end

function PriorityQueue:MinChild(i)
	if (i * 2) + 1 > self.current_size then
		return i * 2
	else
		if self.heap_pri[i * 2] < self.heap_pri[i * 2 + 1] then
			return i * 2
		else
			return i * 2 + 1
		end
	end
end

function PriorityQueue:_swim()
	-- Swim up on the tree and fix the order heap property.
	local heap_val = self.heap_val
	local heap_pri = self.heap_pri
	local floor = floor
	local i = self.current_size

	while floor(i / 2) > 0 do
		local half = floor(i / 2)
		if heap_pri[i] < heap_pri[half] then
			heap_val[i], heap_val[half] = heap_val[half], heap_val[i]
			heap_pri[i], heap_pri[half] = heap_pri[half], heap_pri[i]
		end
		i = half
	end
end

function PriorityQueue:_sink()
	-- Sink down on the tree and fix the order heap property.
	local size = self.current_size
	local heap_val = self.heap_val
	local heap_pri = self.heap_pri
	local i = 1

	while (i * 2) <= size do
		local mc = self:MinChild(i)
		if heap_pri[i] > heap_pri[mc] then
			heap_val[i], heap_val[mc] = heap_val[mc], heap_val[i]
			heap_pri[i], heap_pri[mc] = heap_pri[mc], heap_pri[i]
		end
		i = mc
	end
end

return PriorityQueue